/**
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  * @file       chassis.c/h
  * @brief      2025赛季哨兵底盘控制任务
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. 完成
  *  V1.1.0     Nov-11-2019     RM              1. 增加底盘功率控制
  *  V2.0.0     Jan-1-2024      罗威昊           1. 添加小陀螺
  *  V3.0.0     Dec-20-2024     冉文治           1. 添加普通导航和变速小陀螺导航
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  */

#ifndef CHASSIS_TASK_H
#define CHASSIS_TASK_H

#include "main.h"
#include "CAN_receive.h"
#include "gimbal_task.h"
#include "pid.h"
#include "remote_control.h"
#include "user_lib.h"
#include "referee.h"

// 任务开始空闲一段时间
#define CHASSIS_TASK_INIT_TIME 					355

// 前后遥控器通道通道号码
#define CHASSIS_X_CHANNEL 						1
// 左右遥控器通道号码
#define CHASSIS_Y_CHANNEL 						0
// 选择底盘状态，开关通道
#define CHASSIS_MODE_CHANNEL					0
// 遥控器前进遥感转化为车体前进速度的比例
#define CHASSIS_VX_RC_SEN 						3/660
// 遥控器左右遥感转化为车体左右速度的比例
#define CHASSIS_VY_RC_SEN 						3/660
// X方向速度滤波参数
#define CHASSIS_ACCEL_X_NUM 					0.1666666667f
// Y方向速度滤波参数
#define CHASSIS_ACCEL_Y_NUM 					0.3333333333f
// 遥感死区
#define CHASSIS_RC_DEADLINE 					10
// 四个轮子将合速度除4
#define MOTOR_SPEED_TO_CHASSIS_SPEED_VX 		0.25f
#define MOTOR_SPEED_TO_CHASSIS_SPEED_VY 		0.25f
#define MOTOR_SPEED_TO_CHASSIS_SPEED_WZ 		0.25f
// 电机距离中心点距离
#define MOTOR_DISTANCE_TO_CENTER 				0.2f
// 底盘任务间隔时间2ms
#define CHASSIS_CONTROL_TIME_MS 				2
// 底盘任务间隔时间0.002s
#define CHASSIS_CONTROL_TIME 					0.002f
// 底盘任务控制频率
#define CHASSIS_CONTROL_FREQUENCE 				500.0f
// M3508转化成底盘速度的比例
#define CHASSIS_MOTOR_RPM_TO_VECTOR_SEN 		0.000415809748903494517209f
// 底盘单个电机最大速度
#define MAX_WHEEL_SPEED 						4.0f
// 底盘运动过程中最大前进速度
#define NORMAL_MAX_CHASSIS_SPEED_X 				5.0f
// 底盘运动过程中最大平移速度
#define NORMAL_MAX_CHASSIS_SPEED_Y 				5.0f
// 底盘小陀螺参数设定
#define CHASSIS_SPIN_LOW_SPEED 					1.5f
#define CHASSIS_SPIN_MAIN_SPEED 				10
#define CHASSIS_SPIN_LOW_SEN 					0.6f

// 底盘电机速度环PID
#define M3505_MOTOR_SPEED_PID_KP 				32000.0f
#define M3505_MOTOR_SPEED_PID_KI 				30.0f
#define M3505_MOTOR_SPEED_PID_KD 				0.0f
#define M3505_MOTOR_SPEED_PID_MAX_OUT 			16000.0f
#define M3505_MOTOR_SPEED_PID_MAX_IOUT 			2500.0f

// 底盘旋转跟随PID
#define CHASSIS_FOLLOW_GIMBAL_PID_KP 			15.0f
#define CHASSIS_FOLLOW_GIMBAL_PID_KI 			0.0f
#define CHASSIS_FOLLOW_GIMBAL_PID_KD 			0.0f
#define CHASSIS_FOLLOW_GIMBAL_PID_MAX_OUT 		10.0f
#define CHASSIS_FOLLOW_GIMBAL_PID_MAX_IOUT 		0.0f

// 大yaw轴角度环
#define RADAR_YAW_MOTOR_ANGLE_PID_KP			35.0f
#define RADAR_YAW_MOTOR_ANGLE_PID_KI			0.0f
#define RADAR_YAW_MOTOR_ANGLE_PID_KD			600.0f
#define RADAR_YAW_MOTOR_ANGLE_PID_MAX_OUT		10.0f
#define RADAR_YAW_MOTOR_ANGLE_PID_MAX_IOUT		0.0f

typedef enum
{
	CHASSIS_VECTOR_FOLLOW_GIMBAL_YAW,		// 遥控模式
	CHASSIS_VECTOR_ROTATION,				// 底盘遥控小陀螺
	CHASSIS_VECTOR_NO_FOLLOW_YAW,			// 底盘挂机模式
	CHASSIS_VECTOR_RAW,						// 底盘初始化模式
	CHASSIS_AUTO_FOLLOW_RADAR_YAW,			// 底盘正常导航模式
	CHASSIS_AUTO_ROTATION,					// 底盘小陀螺导航模式
} chassis_mode_e;

typedef enum
{
	CHASSIS_ONLINE,
	CHASSIS_OFFLINE,
} chassis_state_e;

typedef struct	
{
	const motor_measure_t *chassis_motor_measure;
	fp32 accel;
	fp32 speed;
	fp32 speed_set;
	int16_t give_current;
} chassis_motor_t;

// 大yaw数据结构体
typedef struct
{
    const mit_motor_measure_t *gimbal_mit_motor_measure;
    pid_type_def chassis_radar_yaw_motor_angle_pid;

    fp32 output_torque;		// 输出力矩

} chassis_radar_yaw_motor_t;

typedef struct
{
	const RC_ctrl_t *chassis_RC;							// 底盘遥控器指针
	const gimbal_motor_t *gimbal_yaw_motor;					// 小yaw轴电机指针
	const gimbal_motor_t *gimbal_pitch_motor;				// pitch轴电机数据指针
	const fp32 *chassis_INS_angle;							// 欧拉角指针
	const sup_measure_t *chassis_sup;						// 超级电容结构体
	const ld_ctrl_t *ld_ctrl;								// 雷达数据指针
	const ext_game_robot_status_t *robot_status;
	const ext_robot_hurt_t *robot_hurt;

	chassis_radar_yaw_motor_t chassis_radar_yaw_motor;
	
	chassis_mode_e chassis_mode;							// 底盘控制状态
	chassis_mode_e last_chassis_mode;						// 底盘上次控制状态
	chassis_state_e chassis_state;							// 底盘是否上电
	chassis_motor_t motor_chassis[4];						// 底盘电机数据
	pid_type_def motor_speed_pid[4];						// 底盘电机速度PID
	pid_type_def chassis_angle_pid;							// 底盘跟随角度PID
	pid_type_def buffer_pid;								// 底盘功率PID

	first_order_filter_type_t chassis_cmd_slow_set_vx;		// 一阶低通滤波减缓设定值
	first_order_filter_type_t chassis_cmd_slow_set_vy;		// 一阶低通滤波减缓设定值

	uint32_t chassis_dwt_count;								// 任务时间间隔计数
	fp32 dt;												// 任务间隔时间
  
	fp32 vx;												// 地盘速度,前进方向,前为正
	fp32 vy;												// 地盘速度,左右方向,左为正
	fp32 wz;												// 底盘旋转角速度，逆时针为正
	fp32 vx_set;											// 底盘设定速度 前进方向 前为正
	fp32 vy_set;											// 底盘设定速度 左右方向 左为正
	fp32 wz_set;											// 底盘设定旋转角速度，逆时针为正
	fp32 chassis_relative_angle_set;						// 设置相对云台控制角度
	fp32 chassis_radar_yaw_angle_set;						// 设置雷达yaw轴角度
	fp32 chassis_radar_yaw_add_angle_set;					// 设置雷达yaw轴角度增加量
	fp32 vx_max_speed;										// 前进方向最大速度
	fp32 vx_min_speed;										// 后退方向最大速度
	fp32 vy_max_speed;										// 左方向最大速度
	fp32 vy_min_speed;										// 右方向最大速度
	fp32 chassis_yaw;										// 底盘yaw轴角度
	fp32 chassis_pitch;										// 底盘pitch轴角度
	fp32 chassis_roll;										// 底盘roll轴角度
	fp32 chassis_spin_change_sen;							// 底盘小陀螺角速度
	fp32 chassis_radar_angle;								// 雷达yaw轴角度

	fp32 last_robot_HP;

} chassis_move_t;

extern chassis_move_t chassis_move;

/**
  * @brief          底盘任务,间隔2ms
  * @author         冉文治
  * @param          pvParameters: 空
  * @retval         none
  */
extern void chassis_task(void const *pvParameters);

/**
  * @brief          根据遥控器通道值,计算纵向和横向移动速度
  * @author         冉文治
  * @param          vx_set: 纵向速度指针
  * @param          vy_set: 横向速度指针
  * @param          chassis_move_rc_to_vector: "chassis_move"变量指针
  * @retval         none
  */
extern void chassis_rc_to_control_vector(fp32 *vx_set, fp32 *vy_set, chassis_move_t *chassis_move_rc_to_vector);

#endif
